function varargout = MOMCANUI(varargin)
% MOMCANUI MATLAB code for MOMCANUI.fig
%      MOMCANUI, by itself, creates a new MOMCANUI or raises the existing
%      singleton*.
%
%      H = MOMCANUI returns the handle to a new MOMCANUI or the handle to
%      the existing singleton*.
%
%      MOMCANUI('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in MOMCANUI.M with the given input arguments.
%
%      MOMCANUI('Property','Value',...) creates a new MOMCANUI or raises the
%      existing singleton*. Starting from the left, property value pairs are
%      applied to the GUI before MOMCANUI_OpeningFcn gets called. An
%      unrecognized property name or invalid value makes property application
%      stop. All inputs are passed to MOMCANUI_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help MOMCANUI
% Last Modified by GUIDE v2.5 10-Mar-2025 16:14:15

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @MOMCANUI_OpeningFcn, ...
                   'gui_OutputFcn',  @MOMCANUI_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end
if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT


% --- Executes just before MOMCANUI is made visible.
function MOMCANUI_OpeningFcn(hObject, eventdata, handles, varargin)
% GUI initialization function, executed before interface is displayed
% Input parameters:
%   hObject    - GUI object handle
%   eventdata  - Reserved parameter for future versions
%   handles    - Structure containing all GUI control handles
%   varargin   - Command line variable parameter list

% Set GUI output parameters
handles.output = hObject;

% Initialize power supply button state
set(handles.power_supply, 'BackgroundColor', get(0,'defaultUicontrolBackgroundColor'));
set(handles.power_supply, 'Value', 0);

% Update handles structure
guidata(hObject, handles);

% Initialize global variables
global CanOpened;
CanOpened = 0;              % CAN communication status flag

% Initialize LED2 to red (power off state)
set(handles.LED2, 'ForegroundColor', 'red');

global LISTBOXNUM;
LISTBOXNUM = 0;             % Listbox counter

% Load configuration files
load('MOM.mat');            % Load main configuration file
load('RW_Test_Speed.mat')   % Load flywheel test speed configuration

% Initialize display related variables
global RWSpeed;
RWSpeed = RWSpeed;          % Flywheel speed data
global MOMshow;             % Display data cache
global MOMshow2;
MOMshow2 = MOMshow;        % Display data backup

% Initialize savemat with additional columns for voltage, current, and power
global savemat;
savemat = zeros(1, 24);     % Increased size to accommodate voltage (21), current (22), smoothed current (23), and power (24) columns

% Set interface display controls
global UI_UITab1;
UI_UITab1 = [handles.wheel_info_box];           % Auxiliary display listbox

% Set version information
global VersionStr;
VersionStr = "TY-SPACE     Flywheel Test Software    Ver1.5";

% Set data save path
global UIOpenTime;
global DataSaveDir;
global Time;
Time = sprintf('%i%02i%02i%02i%02i%05.3f', clock);
UIOpenTime = Time;

% Create CUI directory in current project folder if it doesn't exist
if ~exist('CUI', 'dir')
    try
        mkdir('CUI');
    catch ME
        errordlg(['Failed to create CUI directory: ' ME.message], 'Directory Creation Error');
        return;
    end
end

% Create timestamped directory inside CUI folder
DataSaveDir = fullfile('CUI', Time);
try
    if ~exist(DataSaveDir, 'dir')
        mkdir(char(DataSaveDir));   % Create data save directory
    end
catch ME
    errordlg(['Failed to create timestamped directory: ' ME.message], 'Directory Creation Error');
    return;
end

% Initialize logging system
global logfilename;         % Log file name
global logfileID;          % Log file handle
global logcount;           % Log counter
global loglinecount;       % Log file line counter
global logBuffer;          % Log message buffer
global lastFlushTime;      % Last buffer flush time
global LOG_BUFFER_SIZE;    % Buffer size
global LOG_FLUSH_INTERVAL; % Flush interval

% Initialize logging variables
logcount = 0;
loglinecount = 0;
LOG_BUFFER_SIZE = 100;     % Buffer up to 100 messages
LOG_FLUSH_INTERVAL = 5;    % Flush every 5 seconds
logBuffer = cell(LOG_BUFFER_SIZE, 1);
logBuffer{1} = '';
lastFlushTime = now;

% Disable warnings
warning off;

global STATS_WINDOW_SIZE;  % Add new global variable for statistics window size
global STATS_CALC_SIZE;    % Add new global variable for statistics calculation size
global processmat;         % Add new global variable for processed metrics storage

% Initialize statistics window size
STATS_WINDOW_SIZE = 500;  % Set to 500 frames as requested by user
STATS_CALC_SIZE = 25;     % Use 25 most recent points for statistics calculations

% Log successful initialization
try
    LogShow('GUI initialization completed successfully');
catch ME
    warning(['Failed to log initialization: ' ME.message]);
end

% --- Outputs from this function are returned to the command line.
function varargout = MOMCANUI_OutputFcn(hObject, eventdata, handles) 
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;

function re = hexstr2num(instr)
% Convert hexadecimal string to decimal number
data1=[];
[m,~]=size(instr);
for i=1:m
    substr = instr(i,:);
    tran1=textscan(substr,'%02s');
    tran2=tran1{1};
    Fdata = hex2dec(tran2);
    send_data = Fdata';
    data1 = [data1 send_data];
end
re = data1;

% --- Executes on button press in send_order_once.
function send_order_once_Callback(hObject, eventdata, handles)
% hObject    handle to send_order_once (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

data=get(handles.InputVAL,'string');
id = get(handles.InputID,'string');
reframes =5;
retimeout = 200;
val1 = hexstr2num(data);
findata=val1;
Command_Deal(findata,id,reframes,retimeout);


function Res = open_can()
%OPEN_CAN Initialize and open CAN device communication
%   This function opens and initializes the CAN device with specified parameters
%
% Returns:
%   Res = 0: Success - Device opened and initialized successfully
%   Res = 1: Initialization failed - Failed to initialize CAN parameters
%   Res = 2: Device open failed - Could not open CAN device
%
% Global Variables:
%   CanOpened - Flag indicating if CAN device is already open

global CanOpened;

% Initialize return value
Res = 0;

% Define device parameters
Device_type = 4;        % Hardware type identifier
Device_index = 0;       % Device index (first device)
Can_index = 0;         % CAN channel index

% Try to open device if not already opened
if CanOpened == 0
    VRec = VCI_OpenDevice(Device_type, Device_index, Can_index);
else
    VRec = 1;          % Device already opened
end

% If device opened successfully, configure it
if VRec
    % CAN bus configuration parameters
    Timing0 = 0;       % Timing register 0 (Baud rate related)
    Timing1 = 28;      % Timing register 1 (Baud rate related)
    Filter = 1;        % Filter mode
    Mode = 0;          % Normal mode
    Code = 0;          % Acceptance code
    Mask = 4294967295; % Acceptance mask (accept all)
    Reserved = 0;      % Reserved parameter

    % Combine configuration parameters
    InitConfig = [Code Mask Reserved Filter Timing0 Timing1 Mode];

    % Initialize CAN if not already initialized
    if CanOpened == 0
        if VCI_InitCAN(Device_type, Device_index, Can_index, InitConfig) ~= 1
            Res = 1;   % Initialization failed
        else
            CanOpened = 1;  % Set global flag
            Res = 0;        % Success
        end
    end
else
    Res = 2;          % Device open failed
end

function Res = close_can()
global CanOpened;
Res = 0;

Device_type = 4;
Device_index = 0;

if VCI_CloseDevice(Device_type,Device_index)~= 1
    msgbox('Failed to close CAN device','Error');
    Res = 1;
else
    CanOpened=0;
end

function Res = start_can()
% Start CAN communication
% Returns:
%   Res = 0: Success
%   Res = 1: Failed to start CAN
Device_type = 4;
Device_index = 0;
Can_index = 0; 

Res = 0;
if VCI_StartCAN(Device_type, Device_index, Can_index) ~= 1
    msgbox('Failed to strat CAN device', 'Error');
    Res = 1;
end

% --- Executes on button press in ConnectCan.
function ConnectCan_Callback(hObject, eventdata, handles)
% hObject    handle to ConnectCan (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global     savecnt;
savecnt=1;
rec =open_can();

if rec==0
    set(handles.LED1,'ForegroundColor','green');
end

% --- Executes on button press in StartCan.
function StartCan_Callback(hObject, eventdata, handles)
% hObject    handle to StartCan (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
start_can();


function Success=CanSendFrame(id,send_data,dlc)
% Send a CAN frame with standard format
Device_type = 4;
Device_index = 0;
Can_index = 0; 
FrameFormat = 0;
FrameType = 0;
TimeStamp=0;
TimeFlag=0;
SendType=1;
SendNums=1;
Data = send_data(1:dlc);
Reserved=[0 0 0];

Frames=[id TimeStamp TimeFlag SendType FrameType FrameFormat dlc Data Reserved];
try
    Success = VCI_Transmit(Device_type,Device_index,Can_index, Frames,SendNums); % Send CAN frame
catch
    Success = 0; 
end

function Success=CanSendFrameExtern(id,send_data,dlc)

Device_type = 4;
Device_index = 0;
Can_index = 0; 
FrameFormat = 1;
FrameType = 0;
TimeStamp=0;
TimeFlag=0;
SendType=1;
SendNums=1;

Data = send_data(1:dlc);
Reserved=[0 0 0];

Frames=[id TimeStamp TimeFlag SendType FrameType FrameFormat dlc Data Reserved];
try
    Success = VCI_Transmit(Device_type,Device_index,Can_index, Frames,SendNums); % Send CAN frame
catch
    Success = 0; 
end

function DelayMsNoblock(time)
loop  = 100*time;
for i=1:5000
    for j =1:loop
    end
end

function rec_ok_flag = CanRecTimeOutFast(frames,RecTime)
Device_type = 4;
Device_index = 0;
Can_index = 0; 
m=0;
loop =0;
if RecTime==0
    CurRecTime=1;
else
    CurRecTime = RecTime;
end
while m<frames 
    try
        m = VCI_GetReceiveNum(Device_type,Device_index,Can_index);
    catch
    end
    %1ms
    DelayMsNoblock(10);
    loop = loop+1;
    if loop==CurRecTime
        rec_ok_flag =0;
        return;
    end
end
rec_ok_flag =1;


function LogShow(Istr)    
% Write log message with timestamp to file
% Parameters:
%   Istr: Message to log
%
% The function adds timestamp to the message and writes it to the log file
% When line count exceeds 10000, a new log file will be created
global logfilename;
global logfileID;
global logcount;
global loglinecount;
global DataSaveDir;
global logBuffer;         % Buffer for collecting log messages
global lastFlushTime;     % Last time the buffer was written to file
global LOG_BUFFER_SIZE;   % Maximum number of messages in buffer
global LOG_FLUSH_INTERVAL; % Time interval for buffer flush in seconds

% Initialize globals if not already done
if isempty(LOG_BUFFER_SIZE)
    LOG_BUFFER_SIZE = 100;    % Buffer up to 100 messages before writing
end
if isempty(LOG_FLUSH_INTERVAL)
    LOG_FLUSH_INTERVAL = 5;   % Flush buffer every 5 seconds
end
if isempty(logBuffer)
    logBuffer = cell(LOG_BUFFER_SIZE, 1);
    logBuffer{1} = '';
    lastFlushTime = now;
end

% Format timestamp
CurTime = sprintf('%04d-%02d-%02d %02d:%02d:%05.3f', clock);
CurTimeStr = sprintf('Time: %-25s', CurTime);
LogStr = sprintf('%s %s\n', CurTimeStr, Istr);

% Add to buffer
if ~isempty(logBuffer{1})
    % Find first empty slot
    emptyIdx = find(cellfun(@isempty, logBuffer), 1);
    if isempty(emptyIdx)
        % Buffer is full, force a flush
        flushLogBuffer();
        emptyIdx = 1;
    end
    logBuffer{emptyIdx} = LogStr;
else
    logBuffer{1} = LogStr;
end

% Check if it's time to flush the buffer
if (now - lastFlushTime) * 86400 >= LOG_FLUSH_INTERVAL || ...
   ~isempty(find(cellfun(@isempty, logBuffer), 1)) == 0
    flushLogBuffer();
end

% Print to console for debugging (only important messages)
if contains(lower(Istr), 'error') || contains(lower(Istr), 'warning') || ...
   contains(lower(Istr), 'speed statistics') || contains(lower(Istr), 'power measurements')
    fprintf(LogStr);
end

function flushLogBuffer()
% Helper function to flush the log buffer to file
global logBuffer;
global lastFlushTime;
global logfilename;
global logfileID;
global loglinecount;
global DataSaveDir;

% Ensure DataSaveDir exists
if ~exist(DataSaveDir, 'dir')
    try
        mkdir(char(DataSaveDir));
    catch
        DataSaveDir = fullfile(pwd, 'logs');
        if ~exist(DataSaveDir, 'dir')
            mkdir(DataSaveDir);
        end
    end
end

% Check if we need to create a new log file
if isempty(logfilename) || ~ischar(logfilename) || loglinecount >= 10000
    % Close current log file if it's open
    if ~isempty(logfileID) && isnumeric(logfileID) && logfileID > 0
        try
            fclose(logfileID);
        catch
            % Ignore errors on close
        end
    end
    
    % Create new log file name
    currentDateTime = datetime('now');
    dateTimeStr = datestr(currentDateTime, 'yyyy.mm.dd.HHMMSS');
    logfilename = fullfile(DataSaveDir, [dateTimeStr, '-logfile.txt']);
    loglinecount = 0;
end

% Open log file in append mode
try
    logfileID = fopen(logfilename, 'a');
    if logfileID > 0
        % Get non-empty messages from buffer
        validMsgs = logBuffer(~cellfun(@isempty, logBuffer));
        
        % Write all messages at once
        if ~isempty(validMsgs)
            fprintf(logfileID, '%s', [validMsgs{:}]);
            loglinecount = loglinecount + length(validMsgs);
        end
        
        % Clear buffer
        logBuffer = cell(length(logBuffer), 1);
        logBuffer{1} = '';
        
        % Close file
        fclose(logfileID);
        
        % Update last flush time
        lastFlushTime = now;
    end
catch ME
    warning('Error writing to log file: %s - %s', logfilename, ME.message);
end

function [frames,ids,redata,datanum,Res] = Common_Receive()
% Receive CAN frames and process data
%
% Returns:
%   frames   - Number of frames received
%   ids      - Array of frame IDs
%   redata   - Array of received data
%   datanum  - Total number of data bytes
%   Res      - Operation result (1: Success, 0: Failed)
%
% The function receives CAN frames and processes them into separate arrays
% for IDs and data. It also handles special cases for ID 255.

% Initialize return values
datanum = 0;
Res = 1;
redata = [];
ids = [];

% Define CAN device parameters
Device_type = 4;
Device_index = 0;
Can_index = 0;

% Try to receive CAN frames with timeout
try
    [GetLength, Frames] = VCI_Receive(Device_type, Device_index, Can_index, 50, 400);
    
    % Log receive attempt
    LogShow(sprintf('Attempting to receive CAN frames... Timeout: 400ms'));
    
    % Check for valid frame length
    if GetLength == 0 || GetLength == hex2dec('FFFFFFFF')
        Res = 0;
        frames = GetLength;
        LogShow('No frames received or invalid frame length');
        return;
    end
    
    % Process received frames
    frames = GetLength;
    if GetLength > 0
        % Log successful reception
        LogShow(sprintf('Successfully received %d CAN frames', GetLength));
        
        % Pre-allocate arrays for better performance
        ids = zeros(1, GetLength);
        redata = zeros(1, GetLength * 8); % Maximum possible data length
        
        % Process each frame
        for i = 1:GetLength
            CurFrame = Frames(i,:);
            
            % Extract frame components
            ID = CurFrame(1);
            len = CurFrame(7);
            CurFrameData = CurFrame(8:(7+len));
            
            % Log frame details
            frameDataHex = sprintf('%02X ', CurFrameData);
            LogShow(sprintf('Frame %d: ID=0x%X, Length=%d, Data=[%s]', i, ID, len, frameDataHex));
            
            % Store frame data
            ids(i) = ID;
            startIdx = (i-1)*8 + 1;
            redata(startIdx:startIdx+len-1) = CurFrameData;
            datanum = datanum + len;
        end
        
        % Trim excess zeros from redata
        redata = redata(1:datanum);
        
        % Process based on last frame ID
        if ID == 255 && GetLength == 6
            LogShow('Processing special frame with ID 255');
            Data_Analysis(GetLength, Frames);
        else
            LogShow('Processing standard frame data');
            analysis(redata);
        end
    end
catch ME
    % Handle errors
    Res = 0;
    frames = 0;
    LogShow(sprintf('CAN receive error: %s', ME.message));
end

% Log final status
if Res == 1
    LogShow(sprintf('CAN receive completed successfully. Total data bytes: %d', datanum));
else
    LogShow('CAN receive operation failed');
end


function analysis(redata)
% Analyze and process received CAN data
% Parameters:
%   redata: Raw data received from CAN bus
%
% Data format:
%   Bytes 1-2:   Header (58, 255)
%   Byte  3:     Data length
%   Bytes 4-12:  Basic data
%   Bytes 13-24: Float data (3 x 4 bytes)
%   Bytes 25-30: Signed integers (3 x 2 bytes)
%   Bytes 31-40: Timestamp and status
global      savecnt
global      VersionStr
global      UI_UITab1
global      MOMshow
global      MOMshow2
global      savemat

[m,~] = size(redata);
if redata(1) == 58 && redata(2) == 255      
    % Parse basic data
    datalen = redata(3);   
    LogShow(sprintf('Analyzing CAN data: length=%d', datalen));
    
    savemat(savecnt,1) = datalen;
    savemat(savecnt,2) = redata(5) + redata(4)*2^8;      % Parse data
    savemat(savecnt,3) = redata(6);              
    savemat(savecnt,4) = redata(7);             
    savemat(savecnt,5) = redata(8);   
    savemat(savecnt,6) = redata(9);     
    savemat(savecnt,7) = redata(10);    
    savemat(savecnt,8) = redata(11);   
    savemat(savecnt,9) = redata(12);   
    LogShow(sprintf('Basic data parsed: [%d,%d,%d,%d,%d,%d,%d,%d,%d]', savemat(savecnt,1:9)));

    % Parse float data
    try
        DATA1 = typecast(fliplr(uint8([redata(13) redata(14) redata(15) redata(16)])), 'single');
        DATA1 = round(DATA1,4);
        savemat(savecnt,10) = round(DATA1,4);

        % DATA2 = typecast(fliplr(uint8([redata(17) redata(18) redata(19) redata(20)])), 'single');
        % DATA2 = round(DATA2,4);
        % savemat(savecnt,11) = round(DATA2,4);
        % 
        % DATA3 = typecast(fliplr(uint8([redata(21) redata(22) redata(23) redata(24)])), 'single');
        % DATA3 = round(DATA3,4);
        % savemat(savecnt,12) = round(DATA3,4);

        DATA2 = redata(17)*256 + redata(18);
        savemat(savecnt,11) = round(DATA2,4);

        DATA3 = redata(21)*256 + redata(22);
        savemat(savecnt,12) = round(DATA3,4);
        
        LogShow(sprintf('Float data parsed: [%.4f, %.4f, %.4f]', DATA1, DATA2, DATA3));
    catch ME
        LogShow(sprintf('Error parsing float data: %s', ME.message));
    end

    % Process signed integer data
    try
        data = redata(26) + redata(25)*2^8;     
        if bitget(data,16) == 0
            savemat(savecnt,13) = data;
        else
            savemat(savecnt,13) = -bitcmp(data-1,'uint16');      % Handle negative values
        end
        
        DATA4 = typecast(fliplr(uint8([redata(27) redata(28) redata(29) redata(30)])), 'single');
        DATA4 = round(DATA4,4);
        savemat(savecnt,14) = DATA4;

    catch ME
        LogShow(sprintf('Error parsing signed integers: %s', ME.message));
    end

    % Parse timestamp and status data
    try
        DATA4 = redata(19)*256 + redata(20);
        savemat(savecnt,16) = DATA4;
        savemat(savecnt,17) = redata(36) + redata(35)*2^8 + redata(34)*2^16 + redata(33)*2^24;
        savemat(savecnt,18) = redata(39) + redata(38)*2^8 + redata(37)*2^16;  
        savemat(savecnt,19) = redata(40);    
        LogShow(sprintf('Timestamp data parsed: UTC=%d, Status=%d', savemat(savecnt,17), savemat(savecnt,19)));
    catch ME
        LogShow(sprintf('Error parsing timestamp data: %s', ME.message));
    end
end

% Update display
MOMshow{1} = VersionStr;
num = [];
for i = 1:6
    m = (i-1)*15 + 1;
    num = mat2str(round(savemat(savecnt,i), 4));
    x = length(num);
    MOMshow{3}(m:m+x-1) = num;
    num1 = mat2str(round(savemat(savecnt,i+6), 4));
    x1 = length(num1);
    MOMshow{7}(m:m+x1-1) = num1;
    num2 = mat2str(round(savemat(savecnt,i+12), 4));
    x2 = length(num2);
    MOMshow{11}(m:m+x2-1) = num2;
end
m = 1;
num3 = mat2str(savemat(savecnt,19));
x3 = length(num3);
MOMshow{15}(m:m+x3-1) = num3;
savecnt = savecnt + 1;
set(UI_UITab1(1),'String',MOMshow);
MOMshow = MOMshow2;

function Command_Deal_Unified(data, id, reframes, retimeout, command_type)
% Unified command processing function that combines Command_Deal, Command_Deal2, and Command_Deal_Extern
% Parameters:
%   data: Command data to send
%   id: CAN ID
%   reframes: Expected response frames (only used for types that expect response)
%   retimeout: Response timeout (only used for types that expect response)
%   command_type: Command type (1=Standard, 2=Quick, 3=Extended)

% Prepare command type info
cmd_types = {'Standard', 'Quick', 'Extended'};
cmd_type_str = cmd_types{command_type};

% Log command initiation
if command_type == 2
    LogShow(sprintf('Processing %s CAN command: ID=0x%s', cmd_type_str, id));
else
    LogShow(sprintf('Processing %s CAN command: ID=0x%s, Expected frames=%d, Timeout=%d', ...
        cmd_type_str, id, reframes, retimeout));
end

% Calculate frame parameters
[~,slen] = size(data);
send_step = ceil(slen/8);  % Simplified calculation using ceil

% Send frames
remaining_len = slen;
for i = 1:send_step
    % Calculate frame size
    send_byte_num = min(remaining_len, 8);
    remaining_len = remaining_len - send_byte_num;
    
    % Extract data for this frame
    frame_start = 1 + (i-1)*8;
    frame_end = frame_start + send_byte_num - 1;
    frame_data = data(frame_start:frame_end);
    
    % Send frame based on command type
    if command_type == 3
        success = CanSendFrameExtern(hex2dec(id), frame_data, send_byte_num);
    else
        success = CanSendFrame(hex2dec(id), frame_data, send_byte_num);
    end
    
    % Log result
    if success
        LogShow(sprintf('%s frame %d/%d sent successfully: %d bytes', ...
            cmd_type_str, i, send_step, send_byte_num));
    else
        LogShow(sprintf('Failed to send %s frame %d/%d', cmd_type_str, i, send_step));
    end
end

% Handle response for non-quick commands
if command_type ~= 2
    rec_ok_flag = CanRecTimeOutFast(reframes, retimeout);
    if rec_ok_flag == 1
        LogShow(sprintf('%s response timeout check passed', cmd_type_str));
        [frames,ids,recdata,datalen,RecOk] = Common_Receive();
        
        if RecOk == 1
            LogShow(sprintf('%s command completed: received %d frames, %d bytes', ...
                cmd_type_str, frames, datalen));
        else
            LogShow(sprintf('%s command failed: receive error', cmd_type_str));
        end
    else
        LogShow(sprintf('%s command failed: timeout after %d ms', cmd_type_str, retimeout));
    end
else
    LogShow(sprintf('%s command completed - No response expected', cmd_type_str));
end

function Command_Deal(data,id,reframes,retimeout)
% Wrapper for backward compatibility
Command_Deal_Unified(data, id, reframes, retimeout, 1);

function Command_Deal2(data,id,reframes,retimeout)
% Wrapper for quick command (no response expected)
Command_Deal_Unified(data, id, reframes, retimeout, 2);

function Command_Deal_Extern(data,id,reframes,retimeout)
% Wrapper for backward compatibility
Command_Deal_Unified(data, id, reframes, retimeout, 3);

function Data_Analysis(GetLength, Frames)
% Analyze CAN frame data for V_data and A_data
% Parameters:
%   GetLength: Number of frames received
%   Frames: Raw frame data array [N x M]
%   Each group of 6 frames contains data for channel 1
%
% The function processes measurements and updates:
%   V_data: Global variable for voltage
%   A_data: Global variable for current
%   P_data: Global variable for power consumption

global V_data
global A_data
global P_data

% Input validation
if nargin < 2
    LogShow('Error: Missing input parameters');
    return;
end

if isempty(Frames) || GetLength <= 0
    LogShow('Error: Invalid frame data');
    return;
end

% Check if we have complete frame groups (6 frames per group)
if mod(GetLength, 6) ~= 0
    LogShow(sprintf('Warning: Incomplete frame group, received %d frames', GetLength));
    return;
end

% Initialize data variables if not already initialized
if isempty(V_data)
    V_data = 0;
    A_data = 0;
    P_data = 0;
end

try
    % Process each group of 6 frames
    for i = 1:floor(GetLength/6)
        % Calculate base index for current frame group
        base_idx = 1 + 6*(i-1);
        
        % Check frame ID
        ID = Frames(base_idx, 1);
        if ID ~= 255  % 0xFF
            LogShow(sprintf('Warning: Invalid frame ID 0x%02X at group %d', ID, i));
            continue;
        end
        
        % Process V_data (only channel 1)
        V_data = (Frames(base_idx, 11)*256 + Frames(base_idx, 12))/1000;
        
        % Process A_data (only channel 1)
        A_data = (Frames(base_idx+2, 11)*256 + Frames(base_idx+2, 12))/10000;
        
        % Calculate power consumption
        P_data = V_data * A_data;
        
        % Log processed data
        LogShow(sprintf('Processed frame group %d:', i));
        LogShow(sprintf('Voltage: %.4f V', V_data));
        LogShow(sprintf('Current: %.4f A', A_data));
        LogShow(sprintf('Power: %.4f W', P_data));
    end
    
catch ME
    LogShow(sprintf('Error processing frame data: %s', ME.message));
    % Keep last valid data
end

function data = CanSendDataFill(sbuf, slen, realdata)
% Fill CAN send data buffer with real data
% Parameters:
%   sbuf: Send buffer to fill
%   slen: Total length of send buffer
%   realdata: Actual data to be sent
% Returns:
%   data: Filled send buffer

% Calculate number of steps needed to send data
send_step = floor(slen/8);
if mod(slen, 8) ~= 0
    send_step = send_step + 1;
end

% Initialize sequence numbers in buffer
for i = 1:send_step
    sbuf(1 + (i-1)*8) = i-1;
end

% Get length of real data
[~, DataLen] = size(realdata);

% Handle empty data case
if DataLen == 0
    % Keep buffer as is
else
    if DataLen <= 4
        % For small data (<=4 bytes), directly copy to buffer
        sbuf(5:5+DataLen-1) = realdata(1:DataLen);
    else
        % For larger data, handle in segments
        % First segment: copy first 4 bytes
        sbuf(5:8) = realdata(1:4);
        
        % Calculate remaining data and steps
        DataLen = DataLen - 4;
        step = floor(DataLen/7);
        if mod(DataLen, 7) ~= 0
            step = step + 1;
        end
        
        % Process remaining data in 7-byte chunks
        for i = 1:step
            if DataLen >= 7
                % Full chunk (7 bytes)
                bss = 2 + i*8;        % Buffer start index
                bse = 8 + i*8;        % Buffer end index
                ss = 5 + (i-1)*7;     % Source start index
                se = 11 + (i-1)*7;    % Source end index
                
                sbuf(bss-1) = i;      % Set sequence number
                sbuf(bss:bse) = realdata(ss:se);
                DataLen = DataLen - 7;
            else
                % Partial chunk (< 7 bytes)
                bss = 2 + i*8;
                bse = 1 + DataLen + i*8;
                ss = 5 + (i-1)*7;
                se = (4 + DataLen) + (i-1)*7;
                
                sbuf(bss-1) = i;
                sbuf(bss:bse) = realdata(ss:se);
            end
        end
    end
end

% Calculate checksum
sbuf(slen) = mod(sum(sbuf(2:4)) + sum(realdata), 256);
data = sbuf;


% --- Executes on button press in CloseCan.
function CloseCan_Callback(hObject, eventdata, handles)
% hObject    handle to CloseCan (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
rec=close_can();
if rec==0
    set(handles.LED1,'ForegroundColor','red');
end


function InputVal_Callback(hObject, eventdata, handles)
% hObject    handle to InputVal (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of InputVal as text
%        str2double(get(hObject,'String')) returns contents of InputVal as a double


% --- Executes during object creation, after setting all properties.
function InputVal_CreateFcn(hObject, eventdata, handles)
% hObject    handle to InputVal (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

% --- Executes on selection change in wheel_info_box.
function wheel_info_box_Callback(hObject, eventdata, handles)
% hObject    handle to wheel_info_box (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: contents = cellstr(get(hObject,'String')) returns wheel_info_box contents as cell array
%        contents{get(hObject,'Value')} returns selected item from wheel_info_box


% --- Executes during object creation, after setting all properties.
function wheel_info_box_CreateFcn(hObject, eventdata, handles)
% hObject    handle to wheel_info_box (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: listbox controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function InputID_Callback(hObject, eventdata, handles)
% hObject    handle to InputID (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of InputID as text
%        str2double(get(hObject,'String')) returns contents of InputID as a double


% --- Executes during object creation, after setting all properties.
function InputID_CreateFcn(hObject, eventdata, handles)
% hObject    handle to InputID (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes on button press in continuous_telemetry.
function continuous_telemetry_Callback(hObject, eventdata, handles)
% Timer callback function for data collection
global val;
global savemat;
global savecnt;
global t10;
global Time;
global STATS_WINDOW_SIZE;  % Add global variable
global STATS_CALC_SIZE;    % Add global variable for statistics calculation
global DataSaveDir;
global processmat;         % Add global variable for processed metrics

% Get button state
val = get(hObject,'value');

% Validate Hz input
Period1 = str2num(get(handles.InputHZ,'string'));
if isnan(Period1) || Period1 <= 0
    errordlg('Please enter a valid positive number for Hz', 'Input Error');
    set(hObject, 'Value', 0);  % Reset button state
    return;
end
Period = 1/Period1;

if val   
    % Initialize variables
    savecnt = 1;
    if ~isnumeric(savemat)
        savemat = zeros(1, 24);  % Initialize with correct size
    end
    processmat = 0;       % Initialize processed metrics matrix
    Time = sprintf('%i%02i%02i%02i%02i%05.3f', clock);
    
    % Ensure data directory exists
    if ~exist(DataSaveDir, 'dir')
        try
            mkdir(DataSaveDir);
        catch ME
            errordlg(['Failed to create data directory: ' ME.message], 'Directory Creation Error');
            set(hObject, 'Value', 0);  % Reset button state
            return;
        end
    end
    
    % Set button color to green
    set(handles.continuous_telemetry, 'BackgroundColor', 'g');
    
    % Initialize the plot
    cla(handles.flywheel_speed_axes);  % Clear current axes
    hold(handles.flywheel_speed_axes, 'on');
    
    % Set up initial plot elements
    hSpeedLine = plot(handles.flywheel_speed_axes, 0, 0, 'LineWidth', 1.5, 'Color', [0.2 0.6 0.9]);
    hMeanLine = plot(handles.flywheel_speed_axes, [0 0], [0 0], '--', 'Color', [0.2 0.8 0.2], 'LineWidth', 1);
    hMaxLine = plot(handles.flywheel_speed_axes, [0 0], [0 0], ':', 'Color', [0.8 0.2 0.2], 'LineWidth', 1);
    hMinLine = plot(handles.flywheel_speed_axes, [0 0], [0 0], ':', 'Color', [0.8 0.2 0.2], 'LineWidth', 1);
    hTrendLine = plot(handles.flywheel_speed_axes, 0, 0, '--', 'LineWidth', 1, 'Color', [0.8 0.3 0.3]);
    
    % Create statistics text box with initial values
    hStatsText = text(handles.flywheel_speed_axes, 0, 0, '', ...
         'BackgroundColor', [1 1 1], ...
         'EdgeColor', [0.5 0.5 0.5], ...
         'LineWidth', 1, ...
         'FontName', 'Consolas', ...
         'FontSize', 9, ...
         'VerticalAlignment', 'top', ...
         'HorizontalAlignment', 'left', ...
         'Margin', 5);
    
    % Set dynamic title
    title(handles.flywheel_speed_axes, sprintf('Flywheel Speed Profile (Window: %d frames, Stats: %d points)', STATS_WINDOW_SIZE, STATS_CALC_SIZE), 'FontSize', 11, 'FontWeight', 'bold');
    
    % Add legend
    legend(handles.flywheel_speed_axes, [hMeanLine, hMaxLine, hTrendLine], ...
           {'Mean Speed', 'Max/Min', 'Trend Line'}, ...
           'Location', 'northeast', 'FontSize', 9, 'Box', 'on');
    
    % Store handles for access in callback function
    handles.hSpeedLine = hSpeedLine;
    handles.hMeanLine = hMeanLine;
    handles.hMaxLine = hMaxLine;
    handles.hMinLine = hMinLine;
    handles.hTrendLine = hTrendLine;
    handles.hStatsText = hStatsText;
    
    guidata(hObject, handles);
    
    % Create and start timer
    t10 = timer('Period', Period, ...
                'ExecutionMode', 'FixedRate', ...
                'TimerFcn', {@update_plot_callback, handles});
    start(t10);
else
    % Stop and delete the timer
    if ~isempty(t10) && isvalid(t10)
        stop(t10);
        delete(t10);
    end
    
    % Set button color to red
    set(handles.continuous_telemetry, 'BackgroundColor', 'r');
    
    % Save final data file in the timestamped directory
    if exist('Time', 'var') && exist('DataSaveDir', 'var')
        try
            save(fullfile(DataSaveDir, [Time, '_final.mat']), 'savemat');
            save(fullfile(DataSaveDir, [Time, '_process_final.mat']), 'processmat');
            LogShow('Saved final data and processed metrics files');
        catch ME
            LogShow('Failed to save final data');
        end
    end
    
    savecnt = 1;
end

function update_plot_callback(obj, event, handles)
% Timer callback function to update plot
global savemat;
global STATS_WINDOW_SIZE;
global STATS_CALC_SIZE;    
global savecnt;
global DataSaveDir;
global Time;
global processmat;

% Call the telemetry function for speed data
remote_telemetry_Callback([], [], handles);

% Calculate speed difference and store it in column 20
if size(savemat, 1) > 1
    current_speed = savemat(end, 10);  % Current speed from column 10
    target_speed = savemat(end, 13);   % Target speed from column 13
    savemat(end, 20) = current_speed - target_speed;  % Store speed difference in column 20
    
    % Log speed difference calculation
    if mod(size(savemat, 1), 50) == 0  % Log every 50 samples
        LogShow(sprintf('Speed difference calculated: Current=%.2f, Target=%.2f, Diff=%.2f', ...
            current_speed, target_speed, savemat(end, 20)));
    end
end

% Update plot if we have data
if size(savemat, 1) > 1
    % Get all speed data
    total_samples = size(savemat, 1);
    speed_all = savemat(:, 10);
    
    % Create sliding window for plotting
    % If total samples is less than window size, use all data
    % Otherwise, use only the most recent STATS_WINDOW_SIZE samples
    if total_samples <= STATS_WINDOW_SIZE
        % Less than window size - use all data for plotting
        plot_start_idx = 1;
        display_data = speed_all;
        frameVector = 1:total_samples;  % Start frame numbers from 1
    else
        % More than window size - use sliding window of most recent data
        plot_start_idx = total_samples - STATS_WINDOW_SIZE + 1;
        display_data = speed_all(plot_start_idx:total_samples);
        
        % Create frame vector that shows actual frame numbers (not starting from 1)
        frameVector = plot_start_idx:total_samples;
    end
    
    % Get the most recent N points for statistics calculation
    % This may be different from the display window size
    stats_start_idx = max(1, total_samples - STATS_CALC_SIZE + 1);
    speed_recent = speed_all(stats_start_idx:total_samples);
    
    % Save data every 500 telemetry cycles
    if mod(total_samples, 500) == 0
        backup_filename = fullfile(DataSaveDir, sprintf('%s_backup_%d.mat', Time, total_samples));
        save(backup_filename, 'savemat');

        % Also save processed metrics
        process_backup_filename = fullfile(DataSaveDir, sprintf('%s_process_backup_%d.mat', Time, total_samples));
        save(process_backup_filename, 'processmat');

        LogShow(sprintf('Auto saved data at sample %d to files: %s and %s', total_samples, backup_filename, process_backup_filename));
    end
    
    % Calculate statistical metrics using most recent N points for statistics
    meanSpeed = mean(speed_recent);
    stdSpeed = std(speed_recent);
    varSpeed = var(speed_recent);
    maxSpeed = max(speed_recent);
    minSpeed = min(speed_recent);
    rmsSpeed = sqrt(mean(speed_recent.^2));  % Modified RMS calculation to handle negative values
    rangeSpeed = maxSpeed - minSpeed;
    
    % Store metrics in processmat
    if isnumeric(processmat) && all(size(processmat) == [1 1]) && processmat == 0
        % First time initialization with column headers
        processmat = zeros(1, 8); % Preallocate for frame, mean, std, var, max, min, rms, range
    end
    
    processmat(savecnt, 1) = total_samples;      % Current frame number
    processmat(savecnt, 2) = meanSpeed;          % Mean speed
    processmat(savecnt, 3) = stdSpeed;           % Standard deviation
    processmat(savecnt, 4) = varSpeed;           % Variance
    processmat(savecnt, 5) = maxSpeed;           % Maximum speed
    processmat(savecnt, 6) = minSpeed;           % Minimum speed
    processmat(savecnt, 7) = rmsSpeed;           % RMS speed
    processmat(savecnt, 8) = rangeSpeed;         % Range of speed
    
    % Calculate plot limits with padding using only displayed data
    yMin = min(display_data) * 1.05;  % Modified for negative values
    yMax = max(display_data) * 1.05;
    if yMin > 0
        yMin = yMin * 0.95;  % Only reduce positive minimum
    end
    if yMax < 0
        yMax = yMax * 0.95;  % Only reduce negative maximum
    end
    
    % Update main speed line with displayed data
    set(handles.hSpeedLine, 'XData', frameVector, 'YData', display_data);
    
    % Update mean, max, and min lines across displayed range
    set(handles.hMeanLine, 'XData', [frameVector(1) frameVector(end)], 'YData', [meanSpeed meanSpeed]);
    set(handles.hMaxLine, 'XData', [frameVector(1) frameVector(end)], 'YData', [maxSpeed maxSpeed]);
    set(handles.hMinLine, 'XData', [frameVector(1) frameVector(end)], 'YData', [minSpeed minSpeed]);
    
    % Update trend line with displayed data only
    windowSize = min(10, length(display_data));  % Adjust window size for smoothing
    smoothSpeed = movmean(display_data, windowSize);
    set(handles.hTrendLine, 'XData', frameVector, 'YData', smoothSpeed);
    
    % Update statistics text with sign-aware formatting
    statsStr = sprintf('Stats (N=%d) | Max: %+.2f | Min: %+.2f | Mean: %+.2f | Std: %.2f | RMS: %.2f | Range: %.2f', ...
                   length(speed_recent), maxSpeed, minSpeed, meanSpeed, ...
                   stdSpeed, rmsSpeed, rangeSpeed);
    
    % Calculate text position using displayed data range
    xRange = frameVector(end) - frameVector(1);
    xPos = frameVector(1) + xRange * 0.02;
    % Adjust yPos based on whether we have negative values
    if yMax > 0
        yPos = yMax - (yMax - yMin) * 0.03;
    else
        yPos = yMax * 0.97;  % Position text near top for negative ranges
    end
    
    % Update text position and content
    set(handles.hStatsText, 'Position', [xPos, yPos, 0], 'String', statsStr);
    
    % Update axis limits to match the current frame window
    xlim(handles.flywheel_speed_axes, [frameVector(1) frameVector(end)]);
    ylim(handles.flywheel_speed_axes, [yMin yMax]);
    
    % Update plot title to show frame range and stats calculation size
    title(handles.flywheel_speed_axes, sprintf('Flywheel Speed Profile (Frames %d-%d, Stats N=%d)', ...
        frameVector(1), frameVector(end), STATS_CALC_SIZE), 'FontSize', 11, 'FontWeight', 'bold');
    
    % Force immediate drawing
    drawnow;

    % Log statistical information
    LogShow(sprintf('Speed Statistics (N=%d) - Mean: %.2f RPM, Std Dev: %.2f RPM, Variance: %.2f RPM?', ...
                   length(speed_recent), meanSpeed, stdSpeed, varSpeed));
end

function InputVAL_Callback(hObject, eventdata, handles)
% hObject    handle to InputVAL (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of InputVAL as text
%        str2double(get(hObject,'String')) returns contents of InputVAL as a double


% --- Executes during object creation, after setting all properties.
function InputVAL_CreateFcn(hObject, eventdata, handles)
% hObject    handle to InputVAL (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function InputHZ_Callback(hObject, eventdata, handles)
% hObject    handle to InputHZ (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of InputHZ as text
%        str2double(get(hObject,'String')) returns contents of InputHZ as a double


% --- Executes during object creation, after setting all properties.
function InputHZ_CreateFcn(hObject, eventdata, handles)
% hObject    handle to InputHZ (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

% --- Executes on button press in remote_telemetry.
function remote_telemetry_Callback(hObject, eventdata, handles)
% Remote telemetry data request callback
global savemat;

data='00 FF 00 00 00 00 00 00';
id = get(handles.InputID,'string');
reframes =5;
retimeout = 200;
val1 = hexstr2num(data);
findata=val1;
Command_Deal(findata,id,reframes,retimeout);

speed = savemat(end, 10);
logstr = ['Current Speed:', num2str(speed)];

% Speed control core functions
function set_rpm(speed, hObject, eventdata, handles)
% Core speed control function for setting rotation speed
% speed: Target speed (rpm)
% hObject, eventdata, handles: GUI control handles and event data structure

% Handle negative speed
is_negative = speed < 0;
speed = abs(speed);

% Calculate high byte and low byte
high_byte = floor(speed/256);
if is_negative
    high_byte = high_byte + 128; % Set positive/negative flag bit
end
low_byte = mod(speed, 256);
data = sprintf('0A 1B 03 %02X %02X 00 00 00', high_byte, low_byte);

% Get ID
id = get(handles.InputID,'string');
reframes = 5;
retimeout = 200;

% Parse data
val1 = hexstr2num(data);
findata = val1;
Command_Deal(findata, id, reframes, retimeout);

function set_rpm_multi(speed, is_negative, hObject, eventdata, handles)
% Core speed control function for setting multiple motor speeds
% speed: Target speed (rpm)
% is_negative: Negative speed flag
% hObject, eventdata, handles: GUI control handles and event data structure

% Calculate high byte and low byte
high_byte = floor(speed/256);
if is_negative
    high_byte = high_byte + 128; % Set positive/negative flag bit
end
low_byte = mod(speed, 256);
data = sprintf('0A 1B 03 %02X %02X 00 00 00', high_byte, low_byte);

% Get ID
ids = {'000000E0', '000000E8', '000000F0', '000000F8'};
reframes = 5;
retimeout = 200;

% Parse data
val1 = hexstr2num(data);
findata = val1;

% Send data to multiple motors
for i = 1:length(ids)
    Command_Deal2(findata, ids{i}, reframes, retimeout);
end

% --- Executes during object creation, after setting all properties.
function flywheel_speed_axes_CreateFcn(hObject, eventdata, handles)
% hObject    handle to flywheel_speed_axes (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Set up flywheel_speed_axes properties
grid(hObject, 'on');
xlabel(hObject, 'Frame Number', 'FontSize', 10);
ylabel(hObject, 'Speed (RPM)', 'FontSize', 10);

% Set grid properties
set(hObject, 'XGrid', 'on');
set(hObject, 'YGrid', 'on');
set(hObject, 'GridAlpha', 0.3);
set(hObject, 'Box', 'on');

% Set axis limits
xlim(hObject, [0 100]);
ylim(hObject, [0 100]);

% Set axis colors
set(hObject, 'XColor', [0.3 0.3 0.3]);
set(hObject, 'YColor', [0.3 0.3 0.3]);
set(hObject, 'LineWidth', 1.2);

% Set font properties
set(hObject, 'FontName', 'Arial');
set(hObject, 'FontSize', 9);

% Set title
title(hObject, 'Flywheel Speed Profile', 'FontSize', 11, 'FontWeight', 'bold');

% Note: This function is called during object creation, so it's not necessary to
% call it manually. It's set up in the GUI creation process.


% --- Executes on button press in set_rpm_order.
function set_rpm_order_Callback(hObject, eventdata, handles)
% hObject    handle to set_rpm_order (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get the RPM value from the input field
rpm_str = get(handles.rpm_value, 'String');
rpm = str2double(rpm_str);

% Validate the RPM value
if isnan(rpm)
    % Not a number
    errordlg('Please enter a valid number for RPM', 'Input Error');
    return;
elseif rpm < -8000 || rpm > 8000
    % Outside valid range
    errordlg('RPM value must be between -8000 and 8000', 'Range Error');
    return;
end

% Store the original signed value
original_rpm = rpm;

% Calculate high and low bytes for the command
is_negative = rpm < 0;
rpm = abs(rpm);
high_byte = floor(rpm/256);
if is_negative
    high_byte = high_byte + 128; % Set positive/negative flag bit
end
low_byte = mod(rpm, 256);

% Create command string in hex format
cmd_string = sprintf('0A 1B 03 %02X %02X 00 00 00', high_byte, low_byte);

% Update the Input VAL text field with the command
set(handles.InputVAL, 'String', cmd_string);

% Log the RPM command
LogShow(sprintf('Sending RPM command: %d, Command string: %s', original_rpm, cmd_string));

% Call the set_rpm function with the original signed value
set_rpm(original_rpm, hObject, eventdata, handles);


function rpm_value_Callback(hObject, eventdata, handles)
% hObject    handle to rpm_value (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of rpm_value as text
%        str2double(get(hObject,'String')) returns contents of rpm_value as a double


% --- Executes during object creation, after setting all properties.
function rpm_value_CreateFcn(hObject, eventdata, handles)
% hObject    handle to rpm_value (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes during object creation, after setting all properties.
function axes1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to axes1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Set up axes1 properties
grid(hObject, 'on');
xlabel(hObject, 'Frame Number', 'FontSize', 10);
ylabel(hObject, 'Value', 'FontSize', 10);

% Set grid properties
set(hObject, 'XGrid', 'on');
set(hObject, 'YGrid', 'on');
set(hObject, 'GridAlpha', 0.3);
set(hObject, 'Box', 'on');

% Set axis limits
xlim(hObject, [0 100]);
ylim(hObject, [0 5]);  % Set default range for speed stability metrics

% Set axis colors
set(hObject, 'XColor', [0.3 0.3 0.3]);
set(hObject, 'YColor', [0.3 0.3 0.3]);
set(hObject, 'LineWidth', 1.2);

% Set font properties
set(hObject, 'FontName', 'Arial');
set(hObject, 'FontSize', 9);

% Set title
title(hObject, 'Speed Stability Metrics', 'FontSize', 11, 'FontWeight', 'bold');

% Note: This function is called during object creation, so it's not necessary to
% call it manually. It's set up in the GUI creation process.


% --- Executes during object creation, after setting all properties.
function axes2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to axes2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Set up axes2 properties
grid(hObject, 'on');
xlabel(hObject, 'Frame Number', 'FontSize', 10, 'FontWeight', 'bold');
ylabel(hObject, 'Speed Difference (RPM)', 'FontSize', 10, 'FontWeight', 'bold');

% Set grid properties
set(hObject, 'XGrid', 'on');
set(hObject, 'YGrid', 'on');
set(hObject, 'GridAlpha', 0.3);
set(hObject, 'Box', 'on');
set(hObject, 'GridLineStyle', '-');  % Solid grid lines

% Set axis limits
xlim(hObject, [0 100]);
ylim(hObject, [-100 100]);  % Initial range for speed difference

% Set axis colors
set(hObject, 'XColor', [0.3 0.3 0.3]);
set(hObject, 'YColor', [0.3 0.3 0.3]);
set(hObject, 'LineWidth', 1.2);

% Set font properties
set(hObject, 'FontName', 'Arial');
set(hObject, 'FontSize', 9);

% Set title with enhanced styling
title(hObject, 'Real-time Speed Tracking Error', ...
    'FontSize', 11, ...
    'FontWeight', 'bold', ...
    'Color', [0.2 0.2 0.6]);


% --- Executes during object creation, after setting all properties.
function axes3_CreateFcn(hObject, eventdata, handles)
% hObject    handle to axes3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Set up axes3 properties
grid(hObject, 'on');
xlabel(hObject, 'Frame Number', 'FontSize', 10, 'FontWeight', 'bold');
ylabel(hObject, 'PID Output', 'FontSize', 10, 'FontWeight', 'bold');

% Set grid properties
set(hObject, 'XGrid', 'on');
set(hObject, 'YGrid', 'on');
set(hObject, 'GridAlpha', 0.3);
set(hObject, 'Box', 'on');
set(hObject, 'GridLineStyle', '-');  % Solid grid lines

% Set axis limits
xlim(hObject, [0 100]);
ylim(hObject, [-100 100]);  % Initial range for PID output

% Set axis colors
set(hObject, 'XColor', [0.3 0.3 0.3]);
set(hObject, 'YColor', [0.3 0.3 0.3]);
set(hObject, 'LineWidth', 1.2);

% Set font properties
set(hObject, 'FontName', 'Arial');
set(hObject, 'FontSize', 9);

% Set title with enhanced styling
title(hObject, 'Real-time PID Control Output', ...
    'FontSize', 11, ...
    'FontWeight', 'bold', ...
    'Color', [0.2 0.2 0.6]);


% --- Executes during object creation, after setting all properties.
function axes4_CreateFcn(hObject, eventdata, handles)
% hObject    handle to axes4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Empty function body - reserved for future implementation


% --- If Enable == 'on', executes on mouse press in 5 pixel border.
% --- Otherwise, executes on mouse press in 5 pixel border or over power_supply_measure.
function power_supply_measure_ButtonDownFcn(hObject, eventdata, handles)
% hObject    handle to power_supply_measure (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)


% --- Executes on button press in power_supply.
function power_supply_Callback(hObject, eventdata, handles)
% hObject    handle to power_supply (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get current button state
button_state = get(hObject, 'Value');

if button_state  % Button is pressed (Value = 1)
    % Send power on commands
    send_power_telemetry();
    enable_power_supply();
    set_current_value();
    
    % Set button color to green
    set(hObject, 'BackgroundColor', [0 1 0]);  % RGB for green
    % Set LED2 to green
    set(handles.LED2, 'ForegroundColor', 'green');
else  % Button is released (Value = 0)
    % Send power off commands
    send_power_telemetry();
    disable_power_supply();
    
    % Set button color to red
    set(hObject, 'BackgroundColor', [1 0 0]);  % RGB for red
    % Set LED2 to red
    set(handles.LED2, 'ForegroundColor', 'red');
end

% --- Executes on button press in power_supply_measure.
function power_supply_measure_Callback(hObject, eventdata, handles)
% hObject    handle to power_supply_measure (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

global V_data;
global A_data;
global P_data;

% Send telemetry command to get voltage and current data
send_power_telemetry();

% Format voltage, current and power data for display with 4 decimal places
voltage_str = sprintf('Voltage: %.4f V', V_data);
current_str = sprintf('Current: %.4f A', A_data);
power_str = sprintf('Power: %.4f W', P_data);

% Get control_info_listbox handle and update display
% Always start with a fresh list containing just the header
control_info = {'Power Supply Measurements:'};

% Add voltage, current and power measurements
control_info = [control_info; voltage_str; current_str; power_str];

% Add timestamp
timestamp = sprintf('Measured at: %s', datestr(now, 'HH:MM:SS'));
control_info = [control_info; ''; timestamp];

% Update listbox with new measurements
set(handles.control_info_listbox, 'String', control_info);
% Scroll to the bottom of the listbox
set(handles.control_info_listbox, 'Value', length(control_info));

function send_power_telemetry()
% Send power telemetry command and receive response
global DEVICETYPE;
global DEVICEINDEX;
global CANINDEX;
global powerflag;
global V_data;
global A_data;
global P_data;

% Initialize V_data, A_data and P_data if they don't exist
if isempty(V_data)
    V_data = 0;
    A_data = 0;
    P_data = 0;
end

% Send telemetry command
ID = '000000FF';
control_data = sprintf('EB 50 FF 01 00');
reframes = 5;
retimeout = 200;
val1 = hexstr2num(control_data);
findata = val1;
CanSendFrameExtern(hex2dec(ID), findata, length(findata));

% Wait for response
rec_ok_flag = CanRecTimeOutFast(reframes, retimeout);
if rec_ok_flag == 1
    % Receive and process data
    [frames, ids, redata, datanum, RecOk] = Common_Receive();
    if RecOk == 1
        LogShow(sprintf('Power telemetry received: %d frames', frames));
    else
        LogShow('Failed to receive power telemetry data');
    end
else
    LogShow('Power telemetry timeout');
end

function enable_power_supply()
% Enable power supply command
global DEVICETYPE;
global DEVICEINDEX;
global CANINDEX;
global powerflag;

ID = '000000FF';
control_data = sprintf('EB 60 00 FF FF');
reframes = 5;
retimeout = 200;
val1 = hexstr2num(control_data);
findata = val1;
CanSendFrameExtern(hex2dec(ID), findata, length(findata));

function disable_power_supply()
% Disable power supply command
global DEVICETYPE;
global DEVICEINDEX;
global CANINDEX;
global powerflag;

ID = '000000FF';
control_data = sprintf('EB 60 00 00 00');
reframes = 5;
retimeout = 200;
val1 = hexstr2num(control_data);
findata = val1;
CanSendFrameExtern(hex2dec(ID), findata, length(findata));

function set_current_value()
% Set current value command
global DEVICETYPE;
global DEVICEINDEX;
global CANINDEX;
global powerflag;

ID = '000000FF';
control_data = sprintf('EB 61 FF FF FF FD');
reframes = 5;
retimeout = 200;
val1 = hexstr2num(control_data);
findata = val1;
CanSendFrameExtern(hex2dec(ID), findata, length(findata));


% --- Executes on selection change in control_info_listbox.
function control_info_listbox_Callback(hObject, eventdata, handles)
% hObject    handle to control_info_listbox (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: contents = cellstr(get(hObject,'String')) returns control_info_listbox contents as cell array
%        contents{get(hObject,'Value')} returns selected item from control_info_listbox


% --- Executes during object creation, after setting all properties.
function control_info_listbox_CreateFcn(hObject, eventdata, handles)
% hObject    handle to control_info_listbox (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: listbox controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end
